#include "a_star.h"
#include <cmath>
#include <unordered_set>

double AStarAlgorithm::heuristic(const QPointF &a, const QPointF &b) {
    // 计算欧几里得距离作为启发式函数
    double dx = a.x() - b.x();
    double dy = a.y() - b.y();
    return std::sqrt(dx * dx + dy * dy);
}

QPair<double, QVector<int>> AStarAlgorithm::aStar(int start, int goal, const QVector<QVector<double>> &distanceMatrix, const QVector<QPointF> &geoVector) {
    int n = distanceMatrix.size();
    QVector<double> gScore(n, std::numeric_limits<double>::infinity());
    std::priority_queue<Node_for_Astar> openSet;
    std::unordered_set<int> closedSet;  // 用于跟踪已经处理过的节点

    gScore[start] = 0;
    openSet.push({start, heuristic(geoVector[start], geoVector[goal]), {start}});

    while (!openSet.empty()) {
        Node_for_Astar current = openSet.top();
        openSet.pop();

        // 如果当前节点已经处理过，则跳过
        if (closedSet.find(current.index) != closedSet.end()) {
            continue;
        }

        // 如果找到目标节点，返回结果
        if (current.index == goal) {
            return qMakePair(current.cost, current.path);
        }

        // 将当前节点标记为已处理
        closedSet.insert(current.index);

        // 检查所有邻居节点
        for (int neighbor = 0; neighbor < n; ++neighbor) {
            if (distanceMatrix[current.index][neighbor] > 0) {
                double tentative_gScore = gScore[current.index] + distanceMatrix[current.index][neighbor];
                if (tentative_gScore < gScore[neighbor]) {
                    gScore[neighbor] = tentative_gScore;
                    QVector<int> newPath = current.path;
                    newPath.append(neighbor);
                    double fScore = tentative_gScore + heuristic(geoVector[neighbor], geoVector[goal]);
                    openSet.push({neighbor, fScore, newPath});
                }
            }
        }
    }

    // 如果未找到路径，则返回无限大和空路径
    return qMakePair(std::numeric_limits<double>::infinity(), QVector<int>());
}
